package com.zhdl.modules.autopick;

import cn.hutool.core.util.HexUtil;
import com.google.common.eventbus.Subscribe;
import com.google.common.primitives.Bytes;
import com.project.command.instruction.strategy.process.data.AfterStrategyParam;
import com.project.common.util.ByteHelper;
import com.project.common.util.DelayUtil;
import com.zhdl.common.eventbus.center.EventBusCenter;
import com.zhdl.hardware.io.smartLink.service.IOCmdSendService;
import com.zhdl.hardware.meich_camera.MeichCameraCmdHandleDao;
import com.zhdl.hardware.robot.borunte.repository.impl.BorunteCmdHandleDaoImpl;
import com.zhdl.hardware_modules.entity.DeviceAction;
import com.zhdl.hardware_modules.service.DeviceService;
import com.zhdl.modules.autopick.entity.CameraCoordinate;
import com.zhdl.modules.autopick.entity.Cassette;
import com.zhdl.modules.autopick.entity.Sensor;
import com.zhdl.modules.autopick.entity.WestData;
import com.zhdl.modules.autopick.repository.CameraRepository;
import com.zhdl.modules.autopick.service.*;
import com.zhdl.modules.autopick.web.request.MemDevice;
import com.zhdl.modules.autopick.web.response.NewMemDevice;
import com.zhdl.modules.autopick.web.response.SseEmitterResultVO;
import com.zhdl.modules.autopick.web.response.WestMemDevice;
import com.zhdl.modules.tools.CommonCallback;
import com.zhdl.modules.tools.EventBusTag;
import com.zhdl.modules.tools.InitCassette;
import com.zhdl.modules.tools.PublicTools;
import com.zhdl.network.asclltcp.AsciiAfterService;
import com.zhdl.network.modbus.dto.ModbusDto;
import com.zhdl.network.service.LaserRangingService;
import com.zhdl.network.service.impl.RobotArmServiceImpl;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.scheduling.concurrent.ThreadPoolTaskScheduler;
import org.springframework.stereotype.Component;

import javax.annotation.Resource;
import java.io.File;
import java.io.IOException;
import java.math.BigInteger;
import java.nio.file.Files;
import java.nio.file.StandardCopyOption;
import java.time.Instant;
import java.util.*;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ScheduledFuture;

@Slf4j
@Component
public class NewProcessControl {

    /**
     * 当前流程是否在运行 - 默认未运行
     * true 运行 false 未运行
     */
    public static boolean isRunning = false;
    /**
     * 是否在调试状态
     */
    public static int isTest;
    /**
     * 主进度 流程进度 - 机器人
     * 分支进度 流程进度 - 电机
     */
    public int processR = -1, processM = -1;
    /**
     * 是否初始化
     * 是否初始化完成
     * 是否初始化完成 --- 机器人
     */
    public boolean isInit = false, isInitFinish = false, robotInit = false,isStart = false;
    /**
     * 料盒初始化
     */
    public List<Cassette> cassetteList;
    /**
     * 当前需要添加药物的料斗
     */
    public Cassette current;
    /**
     * pcb板的查询存储
     */
    public List<Byte> pcbStatusDto02 = new ArrayList<>(), pcbStatusDto01 = new ArrayList<>(), pcbStatusDto10 = new ArrayList<>();
    /**
     * 机器人状态 - 需要谁去使用
     */
    public String robotStatusUser = "";
    /**
     * 电机原点状态 - 是否在原点
     */
    public String motorHomeStatus = "";
    /**
     * 临时状态存储--机器人状态
     */
    public Map<String, String> robotStatusM = new ConcurrentHashMap<>();
    /**
     * 电机 - 电机当前位置 - 电机偏差位置
     */
    public int motorCurrentLocation = -1, differentLocation = 0;
    /**
     * 首页信息 - 内存信息
     */
    public MemDevice memDevice = new MemDevice();

//    public NewMemDevice newNemDevice = new NewMemDevice();
    public WestMemDevice newNemDevice = new WestMemDevice();
    public int doorStatus = -1;
    List<WestMemDevice.CheckInfo> checkInfoList = new ArrayList<>();
    /**
     * 首页信息 - 数据信息
     */
    public SseEmitterResultVO sseResp = new SseEmitterResultVO();
    /**
     * ScheduledFuture 存储集合
     */
    @Autowired
    public ThreadPoolTaskScheduler taskScheduler1;
    public WestData westData = new WestData();
    /**
     * 机器人-当前世界坐标
     * 机器人-抓取点坐标
     */
    int[] robotLocationNow = new int[6];
    //2093.91,-43.56,250.00,0.00
    // |2077.98,604.72,250.00,0.00|
    // 2628.76,-136.91,250.00,-90.00|
    // 2631.57,329.97,250.00,-90.00|
    // 2625.01,764.23,250.00,-90.00|
    int[] pickLocation = new int[3];
    private String currentFileName;
    public static int orationType = -1;
    Map<String, ScheduledFuture<?>> mapSF = new HashMap<>();
    Map<String, Integer> ioAddress = new ConcurrentHashMap<>();
    /*
     * ----------------------------------测试结束------------------------------------------------
     */
    @Resource
    private CameraService cameraService;
    @Resource
    private CmdHandleService cmdHandleService;
    @Resource
    private RobotActionService robotActionService;
    @Resource
    private MotorActionService motorActionService;
    @Resource
    private WestDataService westDataService;
    @Resource
    private CameraRepository cameraRepository;
    @Resource
    private LaserRangingService laserRangingService;
    @Resource
    MeichCameraCmdHandleDao meichCameraCmdHandleDao;
    /*
     * ----------------------------------初始化结束------------------------------------------------
     */

    /**
     * 将byte转换为一个长度为8的byte数组，数组每个值代表bit
     */
    public static Byte[] getBitArray(byte b) {
        Byte[] array = new Byte[8];
        for (int i = 7; i >= 0; i--) {
            array[i] = (byte) (b & 1);
            b = (byte) (b >> 1);
        }
        return array;
    }

    /*
     * ----------------------------------测试------------------------------------------------
     */
    @Subscribe
    public void eventApi(EventBusTag event) {
        String tag = event.getTag();
        switch (tag) {
            case "robotInit" -> {
                processR = 2;
                nextIntRobot();
            }
            case "robotPick" -> {
                processR = 4;
                nextIntRobot();
            }
            case "motorLocation" -> cmdHandleService.motorReadyAndRun();
        }
    }

    /**
     * 根据传感器Id 获取料盒Id
     *
     * @param sensorId 传感器Id
     * @return 料盒Id
     */
    public int getCassetteIdBySensorId(int sensorId) {
        /* 临时变量 - 料盒id */
        int cassetteId = -1;
        /* 根据传感器id，判断出料盒id */
        for (Cassette cassette : cassetteList) {
            for (Sensor sensor : cassette.getSensorList()) {
                if (sensor.getId() == sensorId) {
                    cassetteId = cassette.getId();
                    break;
                }
            }
        }
        return cassetteId;
    }

    /**
     * 初始化参数
     */
    public void release() {
        processR = -1;
        processM = -1;
        isRunning = false;
        current = null;
        westData.setRobotDataTwo("0");
        westDataService.addWestData(westData);
    }

    /**
     * 结束流程-判断是否全部完成
     */
    public void processFinish() {
        if (processR >= 15 && processM >= 12) {
            release();
            log.info("流程全部完成");
        }
    }

    /**
     * 从数据库加载设备到内存 - 赋值初始化，不至于为空
     */
    public void BuildMemDev() {
        for (int i = 0; i < 16; i++) {
            pcbStatusDto01.add((byte) 0);
        }
        for (int i = 0; i < 48; i++) {
            pcbStatusDto02.add((byte) 1);
        }
        BuildMemDev2();
    }

    /**
     * 从数据库加载设备到内存 - 将读取到的加载到内存
     */
    public void BuildMemDev2() {

        byte[] array01 = Bytes.toArray(pcbStatusDto01);
        byte[] array02 = Bytes.toArray(pcbStatusDto02);
        byte[] array10 = Bytes.toArray(pcbStatusDto10);
        StringBuffer string01 = new StringBuffer();
        StringBuffer string02 = new StringBuffer();
        StringBuffer string10 = new StringBuffer();
        for (byte b : array01) {
            string01.append(b);
        }
        for (byte b : array02) {
            string02.append(b);
        }
        for (byte b : array10) {
            string10.append(b);
        }
        log.info("IO显示:功能码为01+++++++++++++" + string01);
        log.info("IO显示:功能码为02+++++++++++++" + string02);
        log.info("IO显示:功能码为10+++++++++++++" + string10);
        memDevice.setCoilIO(string01.toString());
        memDevice.setSensorIO(string02.toString());
        sseResp.setMemDevice(memDevice);
        sseResp.setProcessMode(isTest);
    }

    private int cassetteOne = 0, cassetteTwo = 0, cassetteThree = 0, cassetteFour = 0;

    public void testRun(int type) {
        isRunning = true;
        DelayUtil.delayedExecution(10000, new Runnable() {
            @Override
            public void run() {
                if (type == 0) {
                    cassetteOne++;
                } else if (type == 1) {
                    cassetteTwo++;
                } else if (type == 2) {
                    cassetteThree++;
                } else if (type == 3) {
                    cassetteFour++;
                }
                isRunning = false;
            }
        });
    }

    public void openDoor() {
        if (isRunning) {
            for (WestMemDevice.CheckInfo checkInfo : checkInfoList) {
                checkInfo.setStatus(0);
            }
            // 非正常关闭情况下 需要重新初始化 检测
            robotInit = false;
            stopProcess();
        }
        isStart = false;
        doorStatus = 1;
    }

    public void closeDoor() {
        if (robotInit) {
            reStart();
            isStart = true;
            log.info("IO门关闭后的操作");
        } else {
            checkLink();
        }
        doorStatus = -1;
    }


    /**
     * 设置连接状态
     */
    public void checkLink() {
        /* 测试机器人连接 */
        robotActionService.getRobotCoordinate();

        /* 测试电机连接 */
        cmdHandleService.motorLocation();

        /* 测试3D相机连接 */
        meichCameraCmdHandleDao.startCamera(s -> {
            checkInfoList.get(2).setStatus(1);
        }, 1);

        /* 测试IO连接 */
        cmdHandleService.pcbReadInfo();
    }

    @Resource
    private IOCmdSendService deviceService;


    /**
     * 初始化设备 加载到内存
     */
    public void init() {
        /*
            初始化料盒
         */
        cassetteList = InitCassette.cassetteInit();

        DeviceAction byDeviceId = deviceService.findByDeviceId(1);
        cassetteOne = byDeviceId.getC1();
        cassetteTwo = byDeviceId.getC2();
        cassetteThree = byDeviceId.getC3();
        cassetteFour = byDeviceId.getC4();

        /*
            初始化时-注册广播用户
         */
        EventBusCenter.getEventBus().register(this);
        System.out.println("打印料盒基本数据:" + cassetteList);
        meichCameraCmdHandleDao.init();
        /*
            一直查询pcb状态
         */
        taskScheduler1.scheduleWithFixedDelay(() ->
                cmdHandleService.pcbReadInfo(), 1000);
        /*
            一直查询Motor当前位置
         */
        taskScheduler1.scheduleWithFixedDelay(() ->
                cmdHandleService.motorLocation(), 1000);

        /*
            写入空压机开启
         */
        cmdHandleService.ioWriteCoil(0, "FF00");

        /*
            写入夹爪闭合
         */
        cmdHandleService.ioWriteCoil(1, "FF00");

        /*
            写入压板上合
         */
        cmdHandleService.ioWriteCoil(3, "0000");

        /*
            拉杆回收
         */
        cmdHandleService.ioWriteDoubleCoil(6, false);

        /*
            新增-连接状态
         */
        checkInfoList.add(new WestMemDevice.CheckInfo("机器人连接检测", 0));
        checkInfoList.add(new WestMemDevice.CheckInfo("电机连接检测", 0));
        checkInfoList.add(new WestMemDevice.CheckInfo("3D相机连接检测", 0));
        checkInfoList.add(new WestMemDevice.CheckInfo("远程IO连接检测", 1));
        checkLink();

        newNemDevice.setCheckInfos(checkInfoList);
        newNemDevice.setCassetteOne(cassetteOne + "");
        newNemDevice.setCassetteTwo(cassetteTwo + "");
        newNemDevice.setCassetteThree(cassetteThree + "");
        newNemDevice.setCassetteFour(cassetteFour + "");
        newNemDevice.setStatusDoor(doorStatus == 1 ? "1" : "0");
        newNemDevice.setRobotInit(robotInit ? "1" : "0");
        newNemDevice.setStatusStart(isStart ? "1" : "0");
        newNemDevice.setStatusRunning(isRunning ? "1" : "0");
    }

    public void initAll(){
        /*
            判断电机是否进行初始化，如果没有，则电机进行初始化
         */
        if (!isInitFinish) {
            //初始化-电机找1原点
//            processM = 0;
//            nextIntMotor();
            nextIntMotorInit();
            log.info("-电机进行初始化");
        }
        /*
            判断机器人是否进行初始化，如果没有，则机器人进行初始化
         */
        if (!robotInit) {
            robotInitStep = 0;
            nextIntRobotInit();
        }
    }

    /**
     * 广播接收 串口通讯的数据
     *
     * @param event 解析后的数据
     */
    @Subscribe
    public void eventReceiver(AfterStrategyParam event) {
        /* 通信-网口-IO通信 */
        if (event.getPortKey().equals("192.168.1.108:502")) {
//        if (event.getPortKey().equals("127.0.0.1:502")) {
            /* 获取IO指令返回的数据 */
            //此处接收串口或网口客户端回应代码,并处理
            List<ModbusDto> list = event.getNewData();
            ModbusDto pcbDto = list.get(0);
            /* 获取当前发送的指令 */
            String instruction = event.getInstruction().replaceAll(" ", "");
            //功能码02 读取48个离散量数目
            String statusInstruction02 = PublicTools.getString("cmd", "pcbReadDiscreteSignal");
            //功能码01 读取16个离散量数目
            String statusInstruction01 = PublicTools.getString("cmd", "pcbReadCoilSignal");

            newNemDevice.setCheckInfos(checkInfoList);
            newNemDevice.setCassetteOne(cassetteOne + "");
            newNemDevice.setCassetteTwo(cassetteTwo + "");
            newNemDevice.setCassetteThree(cassetteThree + "");
            newNemDevice.setCassetteFour(cassetteFour + "");
            newNemDevice.setStatusDoor(doorStatus == 1 ? "1" : "0");
            newNemDevice.setRobotInit(robotInit ? "1" : "0");
            newNemDevice.setStatusStart(isStart ? "1" : "0");
            newNemDevice.setStatusRunning(isRunning ? "1" : "0");

            //查询pcb当前的状态 000100000006650200000030
            if (instruction.equals(statusInstruction02)) {
                /* 获取当前指令返回的数据 */
                byte[] unitBuff = pcbDto.getUnitBuff();
                /* 对返回的数据进行处理 - 每八位进行高低位反转 */
                List<Byte> list1 = new ArrayList<>();
                for (byte b : unitBuff) {
                    Byte[] bitArray = getBitArray(b);
                    List<Byte> list3 = Arrays.asList(bitArray);
                    Collections.reverse(list3);
                    list1.addAll(list3);
                }
                if (isTest != 1 && isStart) {
                    String ioSensor = "0000000000000000";
                    //判断当前是哪一个料斗
                    if (list1.get(8) == 0) {
                        log.info("判断料斗一，报警");
                        /* West-Data-添加报警信息 */
                        ioSensor = ioSensor.substring(0, 15) + "1";
                        processEntryBySensorId(8);
                    }
                    if (list1.get(14) == 0) {
                        processEntryBySensorId(14);
                        ioSensor = ioSensor.substring(0, 14) + "1" + ioSensor.charAt(15);
                        log.info("判断料斗二，报警");
                    }
                    if (list1.get(20) == 0) {
                        processEntryBySensorId(20);
                        ioSensor = ioSensor.substring(0, 13) + "1" + ioSensor.substring(14, 16);
                        log.info("判断料斗三，报警");
                    }
                    if (list1.get(26) == 0) {
                        processEntryBySensorId(26);
                        ioSensor = ioSensor.substring(0, 12) + "1" + ioSensor.substring(13, 16);
                        log.info("判断料斗四，报警");
                    }

                    List<Byte> listSensor = new ArrayList<>();

                    listSensor.add(list1.get(8));
                    listSensor.add(list1.get(14));
                    listSensor.add(list1.get(20));
                    listSensor.add(list1.get(26));

                    /* 扔入传感器报警状态 */
                    newNemDevice.setListSensor(listSensor);

                    /* 000000011000011000011000011000000100000000000000 */

                    if (list1.get(16) == 1) {
                        openDoor();
//                        if (isRunning) {
//                            for (WestMemDevice.CheckInfo checkInfo : checkInfoList) {
//                                checkInfo.setStatus(0);
//                            }
//                            // 非正常关闭情况下 需要重新初始化 检测
//                            robotInit = false;
//                            stopProcess();
//                        }
//                        isStart = false;
//                        log.info("IO门打开后的操作");
//                        doorStatus = 1;
                    } else {
                        closeDoor();
//                        if (robotInit) {
//                            reStart();
//                            isStart = true;
//                            log.info("IO门关闭后的操作");
//                        } else {
//                            checkLink();
//                        }
//                        doorStatus = -1;
                    }
                    String shiliu = Integer.toHexString(Integer.parseInt(ioSensor, 2));
                    westData.setRobotDataFour("000" + shiliu);
                    westDataService.addWestData(westData);
                }


                /* 将反转后的结果赋值给 io信号读取 */
                this.pcbStatusDto02 = list1;

                //将读取的数据加载到内存
                BuildMemDev2();

                //判断哪一位是否打开
                log.info("========当前为02线圈信号=======");
                for (int i = 0; i < list1.size(); i++) {
                    Byte b = list1.get(i);
                    if (b == 0) {
                        System.out.println("第" + i + "位，未打开");
                    } else {
                        System.out.println("第" + i + "位，当前打开");
                    }
                }

                /* 新增-pcb连接检测 */
                checkInfoList.get(3).setStatus(1);

            }
            //查询pcb当前的状态 000100000006650100000010
            else if (instruction.equals(statusInstruction01)) {
                /* 获取当前指令返回的数据 */
                byte[] unitBuff = pcbDto.getUnitBuff();
                /* 对返回的数据进行处理 - 每八位进行高低位反转 */
                List<Byte> list1 = new ArrayList<>();
                for (byte b : unitBuff) {
                    Byte[] bitArray = getBitArray(b);
                    list1.addAll(Arrays.asList(bitArray));
                }
                /* 将反转后的结果赋值给 io信号读取 */
                this.pcbStatusDto01 = list1;

                //将读取的数据加载到内存
                BuildMemDev2();

                //判断哪一位是否打开
                log.info("========当前为01线圈信号=======");
                for (int i = 0; i < list1.size(); i++) {
                    Byte b = list1.get(i);
                    if (b == 0) {
                        System.out.println("第" + i + "位，未打开");
                    } else {
                        System.out.println("第" + i + "位，当前打开");
                    }
                }

            } else {
                log.info("未匹配到02 or 01的功能码，当前的功能码为:" + event.getPortKey());
            }

        }
        /* 通信-串口-电机通信 */ else if (event.getPortKey().equals("1")) {
            /* 获取电机指令返回的数据 */
            //此处接收串口或网口客户端回应代码,并处理
            List<ModbusDto> list = event.getNewData();
            ModbusDto modbusDto = list.get(0);

            String instruction = event.getInstruction();
            instruction = instruction.replaceAll(" ", "");
            String statusInstruction = "01030B07000277EE";

            /* 新增-电机连接检测 */
            checkInfoList.get(1).setStatus(1);

            log.info("相机识别结果-->{}-->{}," + "newNemDevice-->{}robotInit{}--->isInitFinish{}isRunning---->{}isStart--->{}",
                    pickLocation,RobotArmServiceImpl.UVW[2], newNemDevice.toString(),robotInit,isInitFinish,isRunning,isStart
            );
            log.info("IO门的状态-->{}内存中门的状态--->{}整个IO信号--->{}",pcbStatusDto02.get(16),doorStatus,pcbStatusDto02.toArray());


            //查询电机状态 0B07 010305240001C4CD
            if (instruction.equals(statusInstruction)) {
                /* 判断策略 - 匹配功能码为03的数据进行处理 */
                if (modbusDto.getCmdType() == 3) {
                    //获取响应数据
                    byte[] unitBuff = modbusDto.getUnitBuff();
                    byte[] mPosition = new byte[4];

                    mPosition[2] = unitBuff[0];
                    mPosition[3] = unitBuff[1];
                    mPosition[0] = unitBuff[2];
                    mPosition[1] = unitBuff[3];

                    String s = ByteHelper.bytesArrayToHexString(mPosition);
                    /* West-Data-添加实时数据 */
                    westData.setRobotDataThree(s);
                    westDataService.addWestData(westData);
                    BigInteger bigInteger = new BigInteger(s, 16);
                    motorCurrentLocation = bigInteger.intValue();
                }
            }//判断 如果指令 等于 下面指令(查询当前是否回到原点)
            /* 判断策略 - 匹配指令为查询是否‘到达原点’的数据进行处理 */
            else if (instruction.equals("010330040001CACB")) {
                /* 获取有效数据-到达原点的状态 */
                byte[] unitBuff = modbusDto.getUnitBuff();
                String hexUnitBuff = HexUtil.encodeHexStr(unitBuff);
                log.info("读取状态+++++++++++++++" + hexUnitBuff);

                /* hex：60 电机没有回到原点 eo 电机回到了原点 */
                /* oct：96 电机没有回到原点 224 电机回到了原点 */
                if (hexUnitBuff.equals("00e0")) {
                    motorHomeStatus = "1";
                    log.info("电机找原点-当前电机回到原点");
                } else {
                    motorHomeStatus = "0";
                    log.info("电机找原点-当前电机未回到原点");
                }
            } else {
                log.info("当前指令为:" + instruction +
                        ",没有获取到对应的指令处理");
            }
        }               /* 通信-网口-机器人通信 */ else if (event.getPortKey().equals("192.168.4.4:502")) {
//        }               /* 通信-网口-机器人通信 */ else if (event.getPortKey().equals("127.0.0.1:5020")) {
            /* 获取机器人指令返回的数据 */
            //此处接收串口或网口客户端回应代码,并处理
            List<ModbusDto> list = event.getNewData();
            ModbusDto modbusDto = list.get(0);
            /* 获取机器人发送的指令 */
            String instruction = event.getInstruction().replaceAll(" ", "");

            /* 新增-机器人连接检测 */
            checkInfoList.get(0).setStatus(1);

            /*
                M10	    等待抓料信号	        M20	    抓料完成信号
                M11	    等待放料信号	        M21	    放料完成信号
                M12	    等待回原点信号	        M22	    回原点完成信号
                M110    等待夹料气缸松开到位	M120	通知夹料气缸松开
                M111	等待夹料气缸夹紧到位	M121	通知夹料气缸夹紧
                M112	等待推袋气缸下推到位	M122	通知推袋气缸下推
                M113	等待推袋气缸上回到位	M123	通知推袋气缸上回
                M114	等待抖料动作完成信号	M124	通知抖料动作开始
                A0-BF                       C0-DF
             */
            /* 判断策略 - 匹配功能码为01的数据进行处理 */
            if (modbusDto.getCmdType() == 0x01) {
                /* 获取对应的指令 根据查询的地址进行匹配 */
                String robotStatusM00 = instruction.substring(16, 20);
                switch (robotStatusM00) {
                    //M20-M22
                    case "00A5" -> {
                        robotStatusM.put("M15", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00A8" -> {
                        robotStatusM.put("M20", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00A9" -> {
                        robotStatusM.put("M21", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00AA" -> {
                        robotStatusM.put("M22", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    //M120-M124
                    case "00C8" -> {
                        robotStatusM.put("M120", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00C9" -> {
                        robotStatusM.put("M121", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00CA" -> {
                        robotStatusM.put("M122", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00CB" -> {
                        robotStatusM.put("M123", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    case "00CC" -> {
                        robotStatusM.put("M124", ByteHelper.bytesArrayToHexString(modbusDto.getUnitBuff()));
                    }
                    default -> log.info("robotM" + robotStatusM);
                }
                StringBuilder s1 = new StringBuilder();
                for (String s : robotStatusM.keySet()) {
                    s1.append("-").append(s);
                }
                log.info("当前信号列表为:" + s1);

            }
            /* 获取定义的查询指令进行匹配 */
            String statusInstruction = PublicTools.getString("cmd", "robotReadArmStatus");
            /* 判断策略 - 匹配查询世界坐标指令返回的数据进行处理 */
            if (instruction.equals(statusInstruction)) {
                /* 获取有效数据-机器人的世界坐标 */
                byte[] unitBuff = modbusDto.getUnitBuff();
                String hexUnitBuff = HexUtil.encodeHexStr(unitBuff);

                /* 判断机器人读取出世界坐标的使用者，如果为Init，则进行处理 */
                if (robotStatusUser.equals("robotArmInit")) {
                    /* 对读取到的数据进行处理-处理读取到的世界坐标 */
                    for (int i = 0; i < hexUnitBuff.length(); i++) {
                        if (i % 8 == 0) {
                            String substring = hexUnitBuff.substring(i, i + 8);
                            BigInteger bigInteger = new BigInteger(substring, 16);
                            int i1 = bigInteger.intValue();
                            robotLocationNow[i / 8] = i1;
                        }
                    }
                    /* 机器人流程-进入初始化流程 */
//                    processR = 3;
//                    nextIntRobot();
                }
            }
        } else {
            log.info("本条指令-串口/网口未匹配,对应串口/网口为:" +
                    event.getPortKey() + ",即将重试");
        }

        System.out.println(Instant.now() +
                "监听者1-->回调1,收到事件：Com" + event.getPortKey() +
                "，线程号为：" + Thread.currentThread().getName());
    }

    /**
     * 加药流程_开始入口
     *
     * @param id 传感器id
     */
    public void processEntryBySensorId(int id) {
        /*   根据传感器id，判断料盒id   */
        id = getCassetteIdBySensorId(id);
        /*      开始流程      */
        processEntryByCassetteId(id);
    }

    /**
     * 加药流程_开始入口
     *
     * @param id 料盒Id
     */
    public void processEntryByCassetteId(int id) {

        log.info("==========================当前不为测试模式，自动流程开始了==========================");

        /* 判断初始化是否完成 */
        if (!isInitFinish || !robotInit) {
            log.info("当前初始化状态，motor+" + isInitFinish + ",robot+" + robotInit);
            return;
        }

        /* 判断当前是否有流程正在进行 */
        if (isRunning) {
            return;
        }
        isRunning = true;

        /* 判断是哪一个传感器报警 */
        for (Cassette cassette : cassetteList) {
            if (cassette.getId() == id) {
                current = cassette;
                break;
            }
        }
        log.info("当前料盒id为:" + current.getId() +
                ",即将判断是否有料--------------------->");

        String s1 = ByteHelper.intToHexStr(current.getId() + 1, 4);
        /* West-Data-添加当前缺料位置 */
        westData.setRobotDataTwo(s1);
        westDataService.addWestData(westData);
        processR = 0;
        nextIntRobot();

    }

    public void cassetteCountAdd(){
        int id = current.getId();
        if (id == 0){
            cassetteOne++;
        } else if (id == 1){
            cassetteTwo++;
        } else if (id == 2){
            cassetteThree++;
        } else if (id == 3){
            cassetteFour++;
        }
        deviceService.updateByDeviceId(cassetteOne,cassetteTwo,cassetteThree,cassetteFour);


        log.debug("当前料斗为:{}",current.getId());
    }

    public void cassetteCountZero(){
        cassetteOne = 0;
        cassetteTwo = 0;
        cassetteThree = 0;
        cassetteFour = 0;
        deviceService.updateByDeviceId(cassetteOne,cassetteTwo,cassetteThree,cassetteFour);
    }

    public void testProcess() {
        if (isRunning) {
            return;
        }
        isRunning = true;

        EventBusCenter.getEventBus().register(this);

        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() ->
                cmdHandleService.pcbReadInfo(), 1000);

        mapSF.put("1", scheduledFuture);

//        processR = 1;
//        nextIntRobot();
    }

    int robotInitStep = -1;
    public void nextIntRobotInit(){
        robotInitStep++;
        switch (robotInitStep) {
            case 1 -> {
                /* 当前状态的使用者 机器人是否初始化 */
                robotStatusUser = "robotArmInit";
                /* 发送指令 - 读取机器人坐标 */
                robotActionService.getRobotCoordinate();
                DelayUtil.delayedExecution(1000, new Runnable() {
                    @Override
                    public void run() {
                        nextIntRobotInit();
                    }
                });
            }
            /* 机器人初始化 - 进行初始化动作 */
            case 2 -> {
                /*
                    1. 需要先给机器人 上电上抬位置
                    2. 通知信号 - M12 - 开始初始化
                 */
                robotActionService.robotArmInit(robotLocationNow);
                //0. 读取信号 M22 -- 判断机器人是否初始化完成
                gentRobotSignal("M22", 170, () -> {
                    robotInit = true;
                    robotInitStep = -1;
                    log.info("机器人-初始化完成");
                }, 2000);
            }
        }
    }

    public void nextIntMotorInit(){
        //1. 初始化上电，发送指令 - 电机找原点
        motorActionService.motorInitZero();
        log.info("电机初始化");
        /* 2. 查询电机当前是否到达电机原点 */
        delayMotorInitFinish(() -> {
            isInitFinish = true;
            log.info("西区电机-初始化完成");
        });
    }



    /**
     * 机器人流程-主流程
     */
    public void nextIntRobot() {
        if (!isStart){
            return;
        }
        /* West-Data-添加实时数据 */
        if (processR == 0) {
            westData.setRobotDataOne("0000");
            westDataService.addWestData(westData);
            log.info("待机");
        } else if (processR == 5) {
            westData.setRobotDataOne("0001");
            westDataService.addWestData(westData);
            log.info("正上方");
        } else if (processR == 6) {
            westData.setRobotDataOne("0002");
            westDataService.addWestData(westData);
            log.info("抓料");
        } else if (processR == 7) {
            westData.setRobotDataOne("0003");
            westDataService.addWestData(westData);
            log.info("放料");
        } else if (processR == 9) {
            westData.setRobotDataOne("0004");
            westDataService.addWestData(westData);
            log.info("扔袋");
        }
        processR++;
        switch (processR) {
            /* case 1,2 为相机拍照流程 */
            case 1 -> {
                /* 控制相机拍照 */
               startCamera(s -> {
                   /* 机器人 - 流程开始运动 */
                   processR = 4;
                   nextIntRobot();
               });

            }
            case 2 -> {

            }
            /* case 3,4 为机器人初始化流程 */
            /* 机器人初始化 - 读取机器人世界坐标 */
            case 3 -> {
//                /* 当前状态的使用者 机器人是否初始化 */
//                robotStatusUser = "robotArmInit";
//                /* 发送指令 - 读取机器人坐标 */
//                robotActionService.getRobotCoordinate();
            }
            /* 机器人初始化 - 进行初始化动作 */
            case 4 -> {
                /*
                    1. 需要先给机器人 上电上抬位置
                    2. 通知信号 - M12 - 开始初始化
                 */
//                robotActionService.robotArmInit(robotLocationNow);
//                //0. 读取信号 M22 -- 判断机器人是否初始化完成
//                gentRobotSignal("M22", 170, () -> {
//                    robotInit = true;
//                    log.info("机器人-初始化完成");
//                }, 2000);
            }
            /* case 5 机器人流程正式开始 */
            /* 1. 机器人-机械臂 移动到料袋准备抓料 */
            case 5 -> {
                /*
                    1.1 写入抓料点位置 , 写入抓料点正上方位置
                    1.2 通知信号 - M10 - 开始抓料
                 */
                robotActionService.robotRunToDrug(
                        pickLocation,
                        pickLocation
                );
                // 1.3 张开机械爪 - 等待抓料
                cmdHandleService.ioWriteCoil(1, "0000");
                // 0. 读取信号 M15 -- 等待通知设置实际抓料坐标
//                gentRobotSignal("M15", 165,
//                        this::nextIntRobot, 2000);
                nextIntRobot();
            }
            case 6 -> {
                gentRobotSignal("M121", 201,
                        this::nextIntRobot, 2000);
//                laserRangingService.sendRangingOnce();
//                DelayUtil.delayedExecution(2000, () -> {
//                    robotActionService.robotSetPickDrug(
//                            pickLocation
//                    );
                    // 0. 读取信号 M121 -- 等待通知夹料气缸夹紧

                //});
            }
            /* 2. 机器人-机械臂 等待信号抓具夹紧 */
            case 7 -> {
                /*
                    2.1 IO控制 - 控制气缸夹紧
                    2.2 回应信号 - M111 - 写入气缸夹紧完成
                 */
                robotActionService.robotWriteArmPick();
                //0. 读取信号 M20 -- 等待抓料完成信号
                gentRobotSignal("M20", 168,
                        this::nextIntRobot, 2000);
//                gentRobotSignal("M20", 168,
//                        this::release, 2000);
            }
            /* 3. 机器人-机械臂 等待抓料完成-通知机器人去放料 */
            case 8 -> {
                /*
                    3.1 通知信号 - M11 - 通知放料开始
                 */
                robotActionService.writeM11();
                //0. 读取信号 M124 -- 等待通知抖料
                gentRobotSignal("M124", 204, () -> {
                    nextIntRobot();
                    log.info("测试-进来第一次");
                }, 2000);

            }
            /* 4. 机器人-机械臂 等待抖料信号-抓具抖动 */
            case 9 -> {
                /*
                    4.1 IO控制 - 控制夹具往返抖动4次
                    4.2 回应信号 - M114 - 抖料完成
                 */
                robotActionService.robotArmShake();

                //0. 读取信号 M21 -- 等待放料完成信号
                gentRobotSignal("M21", 169, () -> {
                    nextIntRobot();
                    log.info("测试-进来第二次");
                }, 2000);

                //4.3 写入夹具有料
                //CacheUtil.getSingleTon().putValue("isEmpty", "false");
            }
            /* case 9 电机流程正式开始 */
            /* 5. 机器人-机械臂 等待夹具松开信号 */
            case 10 -> {
                /*
                    电机流程开始
                 */
                processM = 1;
                nextIntMotor();
                /*
                    -- 机器人 -- 等待M120信号 夹具松开
                 */
                gentRobotSignal("M120", 200,
                        this::nextIntRobot, 2000);
            }
            /* 6. 机器人-机械臂 控制夹具松开 */
            case 11 -> {
                /*
                    6.1 IO控制 - 控制夹具松开
                    6.2 回应信号 - M110 - 夹具松开完成
                 */
                robotActionService.robotArmDownBag();
                //读取信号 M122 -- 等待气缸下推动作信号
                gentRobotSignal("M122", 202,
                        this::nextIntRobot, 2000);
            }
            /* 7. 机器人-机械臂 控制气缸下推 */
            case 12 -> {
                /*
                    7.1 IO控制 - 控制气缸下推
                    7.2 回应信号 - M112 - 气缸下推完成
                 */
                robotActionService.robotArmAirDown();
                //读取信号 M123 -- 等待气缸上回动作信号
                gentRobotSignal("M123", 203,
                        this::nextIntRobot, 2000);
            }
            /* 8. 机器人-机械臂 等待IO压板到位 */
            case 13 -> {
                getSensorStatus(1, 0,
                        this::nextIntRobot, 2000);
            }
            /* 9. 机器人-机械臂 控制气缸上回 */
            case 14 -> {
                /*
                    9.1 IO控制 - 控制气缸上回
                    9.2 回应信号 - M113 - 气缸上回完成
                 */
                /* 9.3 IO控制 - 流程结束后，默认关闭机械爪 */
                //压板-气缸上回
                cmdHandleService.ioWriteCoil(3, "0000");
                DelayUtil.delayedExecution(1000, () -> {
                    cmdHandleService.ioWriteCoil(1, "FF00");
                    DelayUtil.delayedExecution(1000, () -> {
                        cmdHandleService.ioWriteCoil(1, "0000");
                        DelayUtil.delayedExecution(1000, () -> {
                            cmdHandleService.ioWriteCoil(3, "FF00");
                            DelayUtil.delayedExecution(1000, () -> {
                                cmdHandleService.ioWriteCoil(3, "0000");
                                DelayUtil.delayedExecution(1000, () -> {
                                    cmdHandleService.ioWriteCoil(1, "FF00");
                                    DelayUtil.delayedExecution(1000, () -> {
                                        cmdHandleService.ioWriteCoil(1, "0000");
                                        DelayUtil.delayedExecution(1000, () -> {
                                            cmdHandleService.ioWriteCoil(3, "FF00");
                                            DelayUtil.delayedExecution(1000, () -> {
                                                cmdHandleService.ioWriteCoil(3, "0000");
                                                DelayUtil.delayedExecution(1000, () -> {
                                                    cmdHandleService.ioWriteCoil(1, "FF00");
                                                    cmdHandleService.robotM113(195, true);
                                                    nextIntRobot();
                                                });
                                            });
                                        });
                                    });
                                });
                            });
                        });
                    });
                });
            }

            case 15 -> {
                DelayUtil.delayedExecution(3000, () -> {
                    processFinish();
                    log.info("机器人-流程结束");
                });
            }

            default -> log.info("机器人-流程出现异常");
        }
    }

    /**
     * 电机流程-分化流程
     */
    public void nextIntMotor() {
        if (!isStart){
            return;
        }
        processM++;
        switch (processM) {
            /* case 1 为电机找原点功能 */
            case 1 -> {
                //1. 初始化上电，发送指令 - 电机找原点
                motorActionService.motorInitZero();
                /* 2. 查询电机当前是否到达电机原点 */
                delayMotorInitFinish(() -> {
                    isInitFinish = true;
                    log.info("西区电机-初始化完成");
                });
            }
            /* case 2 为电机流程正式开始 */
            /* 1. 电机移动 - 第一段 - 推杆伸出位置 */
            case 2 -> {
                //0. 读取当前原点的偏差
                differentLocation = motorCurrentLocation;
                //1.1 设置电机 - 需要运动的脉冲数
                motorActionService.motorOneStep(current.getCassettePosition().getCasRunOpenLid());
                //1.2 然后读取是否到达了位置 -- 电机如果到达位置，则进行下一步
                stepComplete(current.getCassettePosition().getCasRunOpenLid(), this::nextIntMotor);
            }
            /* 2. IO控制 - 推杆 - 伸出 */
            case 3 -> {
                //2.1 IO控制 推杆伸出
                motorActionService.motorGoOpenBar();
                //2.2 然后读取是否伸出到位 -- 32为伸出到位
                getSensorStatus(32, 1, this::nextIntMotor, 2000);
            }
            /* 3. 电机移动 - 第二段 - 打开料盒位置 */
            case 4 -> {
                //0. 读取当前原点的偏差
                differentLocation = motorCurrentLocation;
                //3.1 设置电机 - 需要运动的脉冲数
                motorActionService.motorTwoStep(current.getCassettePosition().getCasOpenLid());
                //3.2 然后读取料盒是否打开到位  判断传感器信号是否打开到位
                //    -- 电机&&传感器 如果到达位置，则进行下一步
                stepComplete(current.getCassettePosition().getCasOpenLid(), () ->
                        getSensorStatus(current.getSensorByType("1"), 1,
                                this::nextIntMotor, 2000));
            }
            /* 4. IO控制 - 推杆&&电磁阀 - 收回推杆，打开电磁阀 */
            case 5 -> {
                //4.1 IO控制 推杆收回 打开电磁阀
                motorActionService.motorThreeStep();
                //4.2 然后读取 杆儿升起来了 && 电磁阀是否打开
                //    -- -- 33为收回到位
                //    -- 传感器&&传感器 如果到达位置，则进行下一步
                getSensorStatus(33, 1, this::nextIntMotor, 2000);
            }
            /* 5. IO控制 - 电磁阀 - 关闭电磁阀 */
            case 6 -> {
                //5.1 当电磁阀打开的时候，已经可以判断 转运料斗为空
                //CacheUtil.getSingleTon().putValue("isEmpty", "false");

                //5.2 IO控制 关闭电磁阀 延时20s再关闭
                DelayUtil.delayedExecution(20000, () -> {
                    motorActionService.motorFourStep();
                    //5.3 延时 2s 再继续运动
                    //5.3 然后读取 电磁阀是否关闭 -- 如果关闭则进行下一步
                    DelayUtil.delayedExecution(2000, this::nextIntMotor);
                });
            }
            /* 6. 电机移动 - 第三段 - 运动到关盖伸杆位置 */
            case 7 -> {
                //0. 读取当前原点的偏差
                differentLocation = motorCurrentLocation;
                //6.1 设置电机 - 需要运动的脉冲数
                motorActionService.motorFiveStep(current.getCassettePosition().getCasRunCloseLid());
                //6.2 然后读取是否到达了位置 -- 电机如果到达位置，则进行下一步
                stepComplete(current.getCassettePosition().getCasRunCloseLid(),
                        this::nextIntMotor);
            }
            /* 7. IO控制 - 推杆 - 伸出 */
            case 8 -> {
                //7.1 IO控制 推杆伸出
                motorActionService.motorBackOpenBar();
                //7.2 然后读取是否伸出到位 -- 32为伸出到位
                getSensorStatus(32, 1, this::nextIntMotor, 2000);
            }
            /* 8. 电机移动 - 第四段 - 关闭料盒位置 */
            case 9 -> {
                //0. 读取当前原点的偏差
                differentLocation = motorCurrentLocation;
                //8.1 设置电机 - 需要运动的脉冲数
                motorActionService.motorSixStep(current.getCassettePosition().getCasCloseLid());
                //8.2 然后读取料盒是否关闭到位  判断传感器信号是否关闭到位
                //    -- 电机&&传感器 如果到达位置，则进行下一步
                stepComplete(current.getCassettePosition().getCasCloseLid(),
                        () -> getSensorStatus(current.getSensorByType("2"), 1,
                                this::nextIntMotor, 2000));
            }
            /* 9. IO控制 - 推杆 - 收回 */
            case 10 -> {
                //9.1 IO控制 推杆收回
                motorActionService.motorBackCloseBar();
                //9.2 然后读取是否收回到位 -- 33为收回到位
                getSensorStatus(33, 1, this::nextIntMotor, 2000);
            }
            /* 10. 电机移动 - 第五段 - 回到原点 */
            case 11 -> {
                //0. 读取当前原点的偏差
                differentLocation = motorCurrentLocation;
                //10.1 设置电机 - 需要运动的脉冲数
                motorActionService.motorSevenStep(current.getCassettePosition().getCasBackHome());
                //10.2 然后读取是否到达了位置 -- 电机如果到达位置，则进行下一步
                stepComplete(current.getCassettePosition().getCasBackHome(), this::nextIntMotor);
                log.info("西区电机-到达原点");
            }
            /* 11. 电机移动 - 电机停止运动 - 等待下一次信号 */
            case 12 -> {
                //11.1 电机到达原点 设置为 伺服状态
                motorActionService.motorEightStep();
                log.info("西区电机-电机流程结束");

                //11.1.5 计算哪个料都加料 次数+1
                cassetteCountAdd();

                //11.2 初始化参数
                processFinish();

            }
            default -> log.info("西区电机-电机流程出现异常");
        }

    }


    /*
     * ---------------------------------机器人状态判断-----------------------------------------
     */

    /**
     * 获取机器人信号
     *
     * @param signalKey      信号
     * @param signalAddress  对应M信号的地址
     * @param statusCallback 回调函数
     * @param timeout        超时时间
     */
    public void gentRobotSignal(String signalKey, int signalAddress, final StatusCallback statusCallback, int timeout) {
        /* 声明循环次数 */
        final int[] times = new int[]{0};
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
            times[0]++;
            if (times[0] == 1) {
                return;
            }
            /* 获取机器人信号实际状态 */
            String robotStatus = robotStatusM.get(signalKey.split("_")[0]);
            log.info("机器人信号-判断当前" + signalKey + "信号,结果为:" + robotStatus + ",预期结果为:" + "01");
            if ("01".equals(robotStatus)) {
                robotStatusM.put(signalKey, "");
                robotActionService.writeFalseM(signalAddress, false);
                statusCallback.statusCallback();
                ScheduledFuture<?> scheduledFuture00 = mapSF.get(signalKey);
                scheduledFuture00.cancel(false);
                mapSF.remove(signalKey);
            } else {
                robotActionService.getRobotM(signalAddress);
                log.info("查询M信号次数" + times[0]);
                if (times[0] > timeout) {
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get(signalKey);
                    scheduledFuture00.cancel(false);
                    mapSF.remove(signalKey);
                    log.info("已经超时，报警信号:" + signalKey);
                }
            }
        }, timeout / 20);
        mapSF.put(signalKey, scheduledFuture);
        ioAddress.put(signalKey, signalAddress);
    }

    public void stopProcess() {
        isRunning = true;
        isStart = false;
        cmdHandleService.robotSetPause(true);
        for (String s : mapSF.keySet()) {
            ScheduledFuture<?> scheduledFuture = mapSF.get(s);
            scheduledFuture.cancel(true);
        }
    }

    public void startProcess() {
        isStart = true;
//        cmdHandleService.robotSetPause(false);
        release();
    }


    /*
     * ---------------------------------电机状态判断-----------------------------------------
     */

    /**
     * 延时获取当前状态 - 电机初始化完成 -delayMotorInitFinish
     */
    public void delayMotorInitFinish(StatusCallback callBack) {
        /* 声明循环次数 */
        final int[] times = new int[]{0};
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
            times[0]++;
            if (times[0] == 1) {
                return;
            }
            // 判断 -- 当前电气位置是否到达
            if (motorHomeStatus.equals("1")) {
                ScheduledFuture<?> scheduledFuture00 = mapSF.get("delayMotorInitFinish");
                scheduledFuture00.cancel(false);
                mapSF.remove("delayMotorInitFinish");
                callBack.statusCallback();
            } else {
                if (times[0] > 5000) {
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get("delayMotorInitFinish");
                    scheduledFuture00.cancel(false);
                    mapSF.remove("delayMotorInitFinish");
                    log.info("delayMotorInitFinish" + "已经超时，报警信号");
                } else if (times[0] % 200 == 0) {
                    cmdHandleService.motorReady();
                    cmdHandleService.motorInit();
                } else {
                    // 读取是否到达原点
                    cmdHandleService.motorArriveZero();
                }
            }
        }, 100);
        mapSF.put("delayMotorInitFinish", scheduledFuture);
    }

    /**
     * 延时判断-判断电机当前是否到位
     *
     * @param motorArriveLocation 电机需要到达的位置
     * @param callback            回调
     */
    public void stepComplete(final int motorArriveLocation, StatusCallback callback) {
        /* 声明循环次数 */
        final int[] times = new int[]{0};
        String motorLocation = "MotorReadLocation" + motorArriveLocation;
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() ->
                DelayUtil.delayedExecution(1000, () -> {
                    //判断误差区间
                    log.info("当前电气位置为:" + motorCurrentLocation +
                            ",预计到达位置为：" + motorArriveLocation +
                            ",当前偏差为:" + differentLocation);
                    if (motorCurrentLocation >= (motorArriveLocation + differentLocation) - 1000
                            && motorCurrentLocation <= (motorArriveLocation + differentLocation) + 1000) {

                        ScheduledFuture<?> scheduledFuture00 = mapSF.get(motorLocation);
                        scheduledFuture00.cancel(false);
                        mapSF.remove(motorLocation);
                        callback.statusCallback();
                    }
                    /* 超时报警 */
                    times[0]++;
                    if (times[0] > 2000) {
                        ScheduledFuture<?> scheduledFuture00 = mapSF.get(motorLocation);
                        scheduledFuture00.cancel(false);
                        mapSF.remove(motorLocation);
                        log.info("已经超时，报警信号:" + motorArriveLocation);
                        /* 如果超时，则发出指令，让电机停止运动 */
                        cmdHandleService.motorStop();
                    }
                }), 100);
        mapSF.put(motorLocation, scheduledFuture);
    }

    /**
     * 获取传感器状态
     *
     * @param sensorKey key键
     * @param value     预期结果
     * @param callBack  回调
     * @param timeOut   超时时间
     */
    public void getSensorStatus(int sensorKey, int value, StatusCallback callBack, int timeOut) {
        /* 声明循环次数 */
        final int[] times = new int[]{0};
        /* 将传感器的key转换为String类型的 */
        String key = String.valueOf(sensorKey);
        ScheduledFuture<?> scheduledFuture = taskScheduler1.scheduleWithFixedDelay(() -> {
            Byte sensor02 = pcbStatusDto02.get(sensorKey);
            log.info("传感器信号-判断当前:-" + sensorKey +
                    "-传感器信号,结果为:-" + sensor02 +
                    "-,预期结果为:" + value);
            //if ((int)sensor02 == value) {
            if (sensor02 == value) {
                callBack.statusCallback();
                ScheduledFuture<?> scheduledFuture00 = mapSF.get(key);
                scheduledFuture00.cancel(false);
                mapSF.remove(key);
            } else {
                /* 超时报警 */
                times[0]++;
                if (times[0] > timeOut) {
                    ScheduledFuture<?> scheduledFuture00 = mapSF.get(key);
                    scheduledFuture00.cancel(false);
                    mapSF.remove(key);
                    log.info("已经超时，报警信号:" + sensorKey);
                }
            }
        }, timeOut / 20);

        mapSF.put(key, scheduledFuture);
    }

    public void reStart() {
        isStart = true;
        startProcess();
    }

    public void initRobot() {
        initAll();
    }
    int reTryCount =1;
    public void startCamera(CommonCallback<String> callback){
        meichCameraCmdHandleDao.startCamera(s -> {
            s = AsciiAfterService.hexToAscii(s);
            s = s.trim();
            log.info("相机识别结果"+s);
            String[] split = s.split(",");
            if (!split[0].equals("1")) {
                log.info("相机识别结果"+s+">>>>split[0]"+split[0]);
                if (split[0].equals("4")){
                    log.info("无料...");
                }else if (split[0].equals("5")){
                    reTryCount++;
                    if (reTryCount>=5){
                        log.info("重试四次后仍然无结果..需要人工介入!!!!!!!");
                    }
                    log.info("无结果...需要重试"+reTryCount);
                    startCamera(callback);
                }
                return;
            }
            reTryCount = 1;
            for (int i = 0; i < 3; i++) {
                pickLocation[i] = (int) (Float.parseFloat(split[i+1]) * 1000);
                if (i==2){
                    pickLocation[i]+=320 * 1000;
                }
            }
            float angel =  (Float.parseFloat(split[6]));
            if (angel<-40){
                angel+=180.0f;
            }
            if (angel>150){
                angel-=180.0f;
            }
            RobotArmServiceImpl.UVW[2] = (int) (angel * 1000);
            callback.statusCallback("1");
//            borunteCmdHandleDao.borunteSetPickPoint(pickLocation);
//            borunteCmdHandleDao.borunteSetPickZ2(pickLocation);
//            borunteCmdHandleDao.borunteWriteSwitch(160, true);
        },reTryCount);
    }
    public interface StatusCallback {
    void statusCallback();
}

}
